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ABSTRACT 


This  report  presents  an  approach  to  point  target  tracking  based 
on  sequential  filtering  techniques.  The  tracking  problem  is  defined 
in  terms  of  a  nonlinear  vector  differential  equation  and  an  appro¬ 
priate  state  vector.  A  Bayesian  formulation  for  the  problem  is 
selected  which  results  in  a  least-squares  filter  solution.  Linear¬ 
ization  techniques  essential  to  this  approach  are  incorporated  into 
the  development  of  the  solution.  A  computer  program  which  im¬ 
plements  the  complete  solution  algorithm  is  presented.  As  part  of 
this  computer  realization,  numerical  integration  of  the  equations 
of  motion  and  numerical  evolution  of  the  estimate  covariance  ma¬ 
trix  are  discussed  in  detail. 
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AN  APPROACH  TO  TARGET  TRACKING, 


1.  INTRODUCTION 

In  the  past  several  years  a  number  of  publications  in  the  control  theory  literature  has  dealt 

with  the  problem  of  estimating  state  variables  associated  with  nonlinear  systems.  Much  of  this 
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work  is  based  on  the  early  linear  estimation  techniques  established  bv  Kalman  and  Huey.  * 

In  an  engagement  between  a  missile  attack  and  a  defense  system,  one  of  the  crucial  modes 
of  the  defense  system  will  be  point  target  tracking.  As  will  be  shown,  the  problem  of  tracking 
a  point  target  with  a  radar  is  a  nonlinear  parameter  estimation  problem.  The  purpose  of  this 
report  is  to  apply  the?  techniques  of  modern  control  theory  to  that  problem.  A  variety  of  topics 
essential  to  understanding  the  philosophy  of  nonlinear,  recursive  estimation  techniques  is  pre¬ 
sented,  with  application  to  a  specific  re-entry  tracking  problem. 

The  problem  considers  a  missile  trajectory  which  may  be  described  by  the  nonlinear  vector 
differential  equation 

x=f(x)  (1-1) 

where  x  -  xjt)  is  an  n-dimcnsional  vector  whose  components  define  the  trajectory  of  the  approach¬ 
ing  missile.  Observations  on  this  target  are  available  in  the  form 

I(tk)  ^  yk  =  i<xk) +  vk  d-2) 

where  h(x)  *s  an  rn-dimcnsional  vector  function  and  yk  is  Gaussian  white  noise.  The  estimator 
xk  of  xk  will  be  required  to  utilize  the  observation  yk  along  with  the  information  inherent  in  £ 
and  h  and  a  previous  estimate  to  obtain  th€*  best  (in  some  sense)  estimate.  In  particular, 

we  will  give  explicit  equations  for  the  problem  and  obtain  a  solution  for  the  case  of  a  seven¬ 
dimensional  state  vector  with  three  position  components,  three  velocity  components,  and  the 
drag-to-weight  ratio.  The  observation  yk  is  four  dimensional  with  range,  azimuth,  elevation, 
and  range  rate  as  components. 

Each  of  the  following  five  sections  presents  a  particular  facet  of  nonlinear  estimation  prob¬ 
lems.  Section  11  presents  the  general  linearization  techniques  which  are  used  to  modify  the  non¬ 
linear  expressions  and  establish  approximate  expressions  amenable  to  computer  processing. 
Section  111  considers  the  specific  problem  of  obtaining  an  estimator  and  its  covariance.  Sec¬ 
tion  IV  briefly  discusses  the  actual  computer  algorithms  used  to  estimate  the  state  variables. 
Section  V  tabulates  the  actual  state  vectors  and  matrices  defined  for  the  ballistic  missile  re¬ 
entry  problem.  Section  VI  treats  in  detail  the  techniques  used  in  the  numerical  integration  of 
the  nonlinear  system  of  equations  (1-1).  Also  described  in  Sec.  VI  is  an  incremental  linearization 
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technique  used  to  update  the  estimate  covariance  matrices.  It  is  this  particular  area  of  co- 
variance  estimation  which  is  least  developed  in  the  field  of  nonlinear  estimation  problems.  Much 
work  remains  to  be  done  before  all  phenomena  involved  in  such  estimation  problems  can  be  fully 
unde  rstood. 


IT.  LINEA  RIZATION  A  BOLT  A  TRAJECTORY  IN  NONLINEAR  SYSTEMS 

In  the  general  nonlinear  estimation  problem  we  are  confronted  with  the  set  of  equations  (1-1) 
and  (1-2).  Unfortunately,  the  ability  to  obtain  closed-form  solutions  to  such  problems  is  severely 
restricted  by  the  techniques  presently  available.  As  a  rule,  we  must  resort  to  numerical  meth¬ 
ods  and  computer  techniques  in  order  to  achieve  satisfactory  solutions  to  such  problems.  This 
section  presents  some  of  the  linearization  techniques  useful  in  preparing  nonlinear  problems  for 
computer  solution. 

Let  the  system  under  consideration  be  described  by  the  vector  differential  equation 

x-f(x)  (2-1) 

who  re 

x_  =  n -dimensional  state  vector 
X  n-dimensional  vector  function  of  x^ 

It  will  be  assumed  that  X  satisfies  conditions  for  unique  solutions  to  (2-1)  to  exist.  A  solu¬ 
tion  starting  at  time  t  and  initial  state  x  will  be  denoted  by 

r>  ()  — o 

X°(t)  =  \(t,  t  ,  x  )  .  (2-2) 

—  —  o  Q 

Equation  (2-1)  can  be  rewritten  in  the  form  of  a  nonlinear  integral  equation 

pt 

x  (t)  =  x  }  +  \  f  [x(t)|  <lr  (2-3) 

^  Q 


which  is  more  convenient  when  solutions  are  obtained  by  numerical  integration,  as  is  the  case 
here.  In  the  subsequent  discussion  it  will  be  assumed  that  such  a  solution  corresponding  to  , 
tQ  has  been  found,  i.e,,  x_  (t)  is  known.  (An  algorithm  for  the  numerical  solution  of  (2-1)  is  given 
in  Sec.  VL ) 

Algorithms  for  the  estimation  of  state  variables  of  dynamic  systems  from  noisy  data  (ob¬ 
servations)  require  that  the  covariance  matrix  of  x°(t)  be  found,  given  the  covariance  matrix  of 
x  .  For  nonlinear  systems  this  is  usually  not  possible  directly,  so  that  linearizations  have  to 
be  introduced.  Thus  the  equation  of  motion  (2-1)  is  linearized  about  the  nominal  trajectory  x°(t). 
Consider  the  effect  a  small  change  in  xq  has  on  x_°(t). 

Let 

x  *  =  x  +  6n 
— o  — O  “O 

and 

x*(t)  =  X°(t)  +  6x(t)  ■=  x(t,  to.  Xq  )  =  x(t.  to,  xo  +  6xo)  . 


Then,  assuming  that  df/ <)x  exists, 


6x(t)  +  higher  order  terms 


v'(t)  f[x*(t)|  f[x°(t)]  + 


af(x) 


3x 


x-x°(t) 


or,  since  x*(t)  x°(t)  +  6x(t), 

Of 

6x(t)  —  6\(t)  1  higher  order  terms 

f-  X°(t)  ■ 


(2-4) 


Since  it  was  assumed  that  x°(t)  is  known,  (2-4)  can  be  rewritten  as 
6x{t)  =  A(t)  6 +  higher  order  terms 


w  he  vt  * 


(2-5) 


Of 

*(t)  *  3x 


o.  . 

X-X  (t) 


Now  for  small  enough  6x,  the  higher  order  terms  can  be  neglected  in  (2-5)  so  that 
6x(t)  =  A(t)  6x(t)  . 


(2-6) 


By  restricting  6x  (  to  be  small  enough,  6x  [and  hence  x*(t)]  can  be  approximated  arbitrarily  close 

to  its  true  value  by  (2-6).  The  basic  assumption  made  in  nonlinear  estimation  problems  can  now 

be  stated.  If  x*(t)  denotes  the  true  state  of  the  system  and  x°(t)  the  estimate  at  time  t,  based 

on  the  estimate  x  at  time  t  ,  then 
— o  o 


P(t)  =  E{jx»(t)  -  x°(t)|  [x*(t)  -  x°(t)l'} 

=  E{[6x(t)l  1 6 x_( t )  1 1 }  .  (2-7) 

If  the  error  in  the  estimate  x°(t)  is  sufficiently  small,  we  can  use  (2-6)  to  evaluate  6x(t).  Letting 
the  transition  matrix  associated  with  (2-6)  be  denoted  by  0(t,t^),  6_x(t)  is  given  by 

6x(t)  =  <£(t,  t  )  6x^  .  (2-8) 

Hence 

P(t)  =  E{|6x(t)l  [6x(t)|'} 

'  E{|^(t.t0)  6^1  tQ)  6xo]’> 

=  0(t.to)  E(6xo6x^)  0'(t,to) 


or 

P(t)  =  <(t,tQ)  P(tQ)  d  1  (t,  tQ)  .  (2-9) 

The  same  result  is  found  by  obtaining  the  differential  equation  for  <Mt,  t  )  directly.  To  do  this, 
note  that  from  (2-8)  it  follows  that 

dx(t) 

— o 

Therefore,  taking  the  partial  derivative  indicated  in  (2-10)  in  (2-1)  yields 
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(2-11) 


or  rewriting  (2-11)  using  (2-10)  yields 
d  a^> 

IiT  *(t-V  =  —  4>lt’to>  ’  ,2-,2) 

— o 

Since  <*>(t,  t  )  is  to  be  evaluated  along  the  nominal  trajectory  x°(t),  [frf(x)/f,*0l  has  to  be  evaluated 
along  that  same  trajectory.  Hence,  using  the  notation  of  (2-5), 

4  <p(t,t  )  =  A(t)  <6(t,  t  )  .  (2-13) 

dt  o  —  o 

liquations  (2-13)  and  (2-6)  are  equivalent  in  that  for  linear  systems,  the  transition  matrix  satisfies 
its  own  differential  equation.  From  (2-13)  it  is  not  apparent  where  approximations  were  intro¬ 
duced,  since  it  was  not  necessary  anywhere  to  neglect  higher  order  terms  as  it  was  for'  (2-6), 
However,  it  is  quite  easy  to  see  that  in  order  to  use  the  solution  of  (2-13)  to  evaluate  the  co¬ 
variance  matrix  P(t),  the  assumption  that  oxjt)  is  small  has  to  be  made  again.  For  in  that  case, 
it  follows  directly  that 

6  x  -  <t>(  t ,  t  )  6  x 

—  o  — o 


and  hence 


P(t)  =  <Mt,  t  )  P(t  )  0'(t,  t  ) 
—  o  —  o  o 


(2-14) 


111.  DERIVATION  OF  THE  P RACKING  EQUATIONS 

In  Sec,  II,  discussion  centered  about  a  system  which  could  be  described  by  a  nonlinear  vector 
differential  equation  (2-1).  In  this  section,  it  will  be  assumed  that  an  exact  solution  to  such  a 
nonlinear  system  of  equations  is  available  and  can  be  expressed  in  the  discrete  form 

*k  =  I(2k-d  +  ilk  •  (3-n 

An  expression  relating  these  state  variables  x^  to  the  observations  may  be  written  as 


ik =  +  ^k 


(3-2) 


A  separate  report  will  discuss  some  implications  of  the  transition  from  the  continuous  to  tin4 
discrete  domain. 

It  is  now  possible  to  derive  an  estimator  for  utilizing  the  observations  y ^  if  the  following 
assumptions  are  satisfied: 


u.  .  v,  -  white,  zero  mean, 

-k  ~k 


E(ukuk  )  =  CJR 


E<  V-k  >  =  l\ 


Gaussian  random  variables  with 
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and  g,  h.  and  the  noise  and  are  such  that  for  incremental  changes,  the  equations 
ftg(x) 


6\  a 
— k  <)x 

3h(  x_) 


x  xi  ^ 
- k- 1 


6^k-i 


6v 


— k  c)x 


x“xk 


6*x 


hold.  This  in  effect  assumes  that  the  x  process  is  Gaussian  with  a  mean  whose  evolution  in 
time  is  given  by  a  nonlinear  equation.  Under  these  assumptions 


P(2k^k'  ;  (2]r)m/21|R  ,1/2  eXP(-I  tyk  -  h<Xk'U  li-1  lYk  "  !K*k>l> 


— k 


Pted'W  *  ,,  n/2  , „ 


(2Tr)"  "ISwk.il 


1/2 


f  1  ,  A  .1 

exp  [-7 


(3-3) 


where 


p(>k|Yk-i' 


X-k,  k-l(-k  — k,  k-1  ^ 


^  I  — k-k,  k- 1— k  +  — k 


l/2  exp  ^  2  ^k  -*-k,  k-l" 


x  <likS  Vg  +  R,)-1  l^k-h(£k  k-1>l> 


n  :  dimension  of  x 


m  dimension  of  the  observation  vector  v. 

-k 

V.  .  -  set  of  all  observations  y.,  i  =  1,  2,  .  .  .  k—  1 
k-1  -j  J 


— k,  k-1 


(3-4) 


(3-5) 


1  =  state  estimate  at  time  t^  ^  based  on  all  observations  y 
j  1, 2,  ...  k  -  1  (i.e.,  Y  p 


-k,  k-1  E  l-k  -k,  k-l'(-k  -k,  k-1*  1 

ah(x) 


H, 


ax 


^k,  k-i 


Following  Ho  and  Lee,  p( >^/ Y is  given  by 

p%|Yk-i'  , 

P<~k  Yk)  =  P(lklYk_i)  P  —  k1— k^ 


(3-6) 
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or 


1  H,  s,  L  4  R 

„>  i  v  k— k,  k- 1— k  k 

! v  J  = - 1 - 


-k'  '  k’  ,n.’2  .  „  :  1/2 


<2*>  Skl  l§k.  k-i 


T,„jV2 

172  exp|-|  ^k-^k11'  fik1 


;r  c,  -l 


l-k  -'-k’1  +  (-k  — k,  k- 1 '  -k,k-l(-k  -k,  k-1  ' 

-[yk-H(2k,k.i»lT(ilkskik-,ilkr  -  iik)_1  Hk-^k.k-k^)  • 


(3-7) 


Because  x_^  occurs  nonlinearly  in  (3-7),  it  is  in  general  not  possible  to  solve  directly  for  x^,  the 
conditional  expectation  of  Xj.*  However,  assuming  that  the  true  state  x^  is  close  to  ^  ^  and 
assuming  appropriate  properties  of  h(x),  the  term  h( x ^ )  can  be  expanded  about  _x^  ^  ^  in  the  form 


A  8h<x) 

IKSk^^k.k-i*  +  ~bT 


A 

- '  -k,  k-1 


(-xk-^k.  k-i> 


(3-8) 


i.e.,  only  up  to  first  order  terms  of  Ja( are  retained  in  its  Taylor  series.  This  implies  that 
in  the  quadratic  form 


lik-i'V1'  ^k1  Hk-^k11 

up  to  second  order  terms  should  be  retained. 

Expanding  the  term  -  h(x^)  up  to  second  order  yields 

Ik  ~  Ll^k'  S  >'k  “  «*k,  k-k  “  — k(-k  ~  K  k-l> 


1 

|_a_ 

r9h(x)  i 

—  (v  -  ^  ) 

2 

cix 

fix  ~k  ~k,  k-1  J 

A 

-^k.  k-11 

[-k  -k,  k-1 


(3-9) 


(3-10) 


Using  (3-10)  and  retaining  all  terms  up  to  second  order  in  ~  ^  ^  yields  for  (3-9) 

fck  -*(sk,i1  K'  (yk  - -  i7k - k-i)|T  ^k1  Hk-^(ik,k-iM 

+  (-k--k,  k-1*  -k  -k  —  k(-k  ~  -k,  k-l'  ~  2  ^k  ~  -(-k,  k-l'' 


* -k  — k'-k  -k,  k-l'  2  (-k  -k,  k-1 


I  <] 

r0h(x)  i 

1 

j  fix 

Ox  “k  “k,  k- 1  J 

A 

-k,  k-l’ 

-1 


Sk  iyk-!j^k,k-i" 


-  j[yk-!i(xkj  k_4)lT  iik  1 


0 

f%(i)  „  1 

0X 

9x  — k  ~  — k,  k-1  ] 

A 

-k,  k-1 

*<*k^k.k-i>  • 


(3-11) 
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By  writing  the  last  two  expressions  in  (3-11)  in  terms  of  the  components  of  the  various  vectors, 
it  can  be  shown  that  they  are  equivalent  to 


(~k  -k,  k-1 


J  /  c) 


ax 


ah(x) 


Ox 


i  %  Iyk-^k.  k-i>i 


A 

-k,  k-1 


*-k  -k,  k-1* 


To  simplify  the  subsequent  analysis,  define 


B  A  — 
-k  3\ 


rah(x)i 

-  1  A 

l  ax  I 

£k  Hk  ^k.k-i") 

-k,  k-1 


,  -1  T  -1 

As  long  as  (S^  —  B^)  is  positive  definite,  (3-7)  can  be  rewritten  as 

p(-k  3  k*  3  °  expl~  2  ^(-k  "  -k,  k-1*  (-k,  k-1  4  -k -k  — k  _ -k*  (-k  ~  -k,  k-1 1 

-2(xk- xk>k_i)r  HkrRk_1  iZk-^k, k-i>i +  [yk-h(£k,k.1nr 


l^k1  -  (-k-k,  k-liik  +  — k1'1*  tZk  k-l»» 


( 'onsider  the  term 


(3-12) 


(3-13) 


(3-14) 


%_1  -  <Hk4,  k-l-k  +  ^k’"1  3  <I!k^k,  k-l-k  +  flk*'1  -k-k,  k-l^lik1 


(3-15) 


which  can  be  rewritten  as 

<^k,  k-lilk  +  ^k1'1  -k-k,  k-i<k-l  +  iik^k^k’ 


x  (S 


-1  T  -1  -1  T  -1 

k,  k-t  +  — k  — k  — k)  iik^k 


( 3-1 5a) 


After  some  matrix  manipulation,  (3-1  5a)  simplifies  to 


^k1  ®k§k.  k-1— if  +  =  -k'-^-kfk-l  +  aftk'o/'  wX1 


(3-16) 


I  Jefine 


Sk'^^k-l  4  -if-k^k  ”  5k) 


Add  and  subtract  from  the  exponent  in  (3-14)  the  term 

•£k-^k.k-l»Tfikfiik(§k.Vl  +HkIk’1LV‘1  -k-k— if —k^  lZk-SSk.k-i’1  •  (3-17) 

Then  the  quadratic  term  in  y^  -  h(£k  ^  |)  becomes 

tyk-^kik.1)iT  Bk'X^kfk-i  +-BkTsk'1)'1  {l-+  W 
x  — if— k"  [yk-^k,k-i>i-^-(3-17>  • 

After  simplifying,  this  reduces  to 


IZk  k-i>lT  Bk'jW-'kX'  IZk-WSk,  k-11 


(3-18) 
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Since  Hr  is  proportional  to  Ur  1  [y  r  —  H(Xr  r  ^)|,  the  term  (3-17)  is  of  third  order  in  Ur  5  [Yr 
h(xR  r  ^1-  Kurthermore.  every  term  in  the  expansion  of  (3-17)  contains  second  order  derivatives 
of  the  components  of  h(x),  so  that  in  (3-18),  the  term  given  by  (3-17)  can  be  neglected.  With  this 
assumption  the  exponent  in  (3-14)  can  be  written  as 


exponent  *  -j  <(  xR  -  2Rj  k. , )  ‘  Sk'\ *k  -  xR  R.1 ) 


2(-k  -k,  k-1 


*  — k^-k  1  Hk-^k.k-l" 


+  l*k  -  ^k,  k-1  "T  -k '-k^k-k  -  k  1  Hk  -  k-l>l> 


(3-19) 


By  multiplying  out  t Be  quadratic  form  below,  we  can  verify  that  (3-19)  can  be  written  as 
exponent. -i[(xk-(£k  k_,  ♦  SkHk,Bk'1  l2k  -  Wsk,  k-l,,}  1 '  Y 


‘MX*  '2k  ■ 

From  (3-20)  it  follows  that  the  estimate  Xr  (the  conditional  mean)  is  given  by 


(3-20) 


^k  =  5k.  k-1  +  Skiik'^k  '  [^k  -  k-1 ’I 


and  the  covariance  of  Xr  is  given  by 


Sk  =  E  HSfc-Sfc)  (5k-V 

I\ q u a t i on  (3-21 )  can  be  r e w r it t e n 


(3-21) 


Sk=[S 


-1 


T  -1  3 

+  R,  H,  -  7- 


t  — k,  k-  1  — k-k  — k  Ox 

or,  using  the  definition  of  Hr, 

-1  a 


f0h(x)]T 

bf|  V  iyk - b(xk 


k-1 


-k,  k-1 


-1 


-k  1  — k,  k-1  Ox 


0h(  x) 


dx 


[yk  -  h<2i>] 


-k,  k-1 


-1 


(3-22) 


and  Xr  is  given  by 


T„  -1 


-k  "  -k,  k-1  + -k-k  — k  &k  — k,  k- 1  ^ 
To  determine  Sr  r  ^ ,  expand  (3-1)  about  \r  ^  to  yield 


(3-23) 


Og 

6x.  ^  -Z 
-k  Ox 


^k-i 


65k-i  +ak 


(3-24) 


where 


62,  A  x  5 

-k-1  -k-1  -k-1 


a2k*3k-g<*k_,> 
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Keeping  only  up  to  first  order  terms  in  the  expansion  of  6x,  yields 


3g  3g  T 

1-(6-k6-k  '  ^  -k,  k-1  ~  a  -k-1  f)T  a  +  ^ 

—  V  —  V 


-k-1 


~k-l 


(3-ZS) 


where  we  use  the  fact  that  is  independent  of  6x^  ^  and  the  mean  of  is  zero.  By  similar 
reasoning,  it  is  easily  established  that 


Cov(yk|Yk- 


H.S.  ,  ,M.r  + 
— k—k,  k-1— k 


^k 


(3-26) 


This  completes  the  derivation.  It  will  be  noted  that,  analogous  to  the  continuous  case,  the  co- 
variance  matrix  given  by  (3-21)  contains  the  additional  term  B^  [defined  by  (2-1  3)|  which  does 
not  occur  if  linear  filtering  theory  is  applied  to  the  linearized  system.  For  this  report,  u^  0 
has  been  assumed  in  later  sections. 


IV.  COMPUTER  REALIZATION  OF  THE  ESTIMATOR 

A  computer  realization  for  (3-23)  is  now  desirable.  Since  (3-1)  is  not  readily  available, 
part  of  the  computational  algorithm  must  be  devoted  to  obtaining  a  solution  to  (2-1).  A  detailed 
discussion  of  this  specific  problem  is  presented  in  Sec.  VI.  The  present  section  will  briefly 
describe  the  complete  program  for  obtaining  t He  state  variable  estimate. 

The  algorithm  used  is  in  fact  a  valid  result  for  three  different  approaches  to  the  estimation 

A  C 

problem  —  that  of  Sec.  Ill  and  those  of  Mowery4  and  Cox.  The  similarity  in  the  solutions  for 
all  three  cases  is  due  to  the  fact  that  the  assumptions  made  in  each  case  reduce  the  problem  in 
the  final  stages  to  minimizing  a  quadratic  form  involving  the  observations,  predicted  values  of 
the  state  vector,  and  functions  of  that  predicted  state.  Also  of  interest  is  the  flexibility  of  pro¬ 
gramming  the  solution  in  two  different  ways  (see  part  C  of  this  section). 

The  equations  describing  the  system  are  given  by 

x  ~  I(x)  [Eq.  (2-l)| 

yk  #  y(tk)  =  h(£k>  +  vk  |Eq.  (3-2)1 

where 

x  -  state  vector 

v,  -  observation  vector  at  time  t, 

^k  k 

x.  =  state  vector  at  time  t, 

— k  k 


f ,  Jh  -  vector  valued  functions 

v,  white,  Gaussian,  zero  mean  noise 

-k  * 


In  Fig.  1,  an  overall  block  diagram  is  given  for  the  routine.  Figures  2  and  3  show  the  detailed 
flow  diagrams  for  the  problem.  The  following  definitions  have  been  used: 

x.  =  state  estimate  (prediction  of  state)  at  time  t.  . ,  based  only 

— k  f 1 ,  k  K+i 

on  the  estimate  at  time  t, 


-kfl 


state  estimate  at  time  t^+1,  based  on  y^+1  and  x^  +  1  k 
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Fig.  1.  Estimation  routine. 
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Fig.  2(a).  Conditional  integration  routine  1. 
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Fig.  2(b).  Conditional  integration  routine  II. 
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1  3-42-10374 


INPUT  FROM  CONDITIONAL  INTEGRATION  ROUTINE  I 


OUTPUT  IN  RADAR  OUTPUT  IN  RECTANGULAR 
COORDINATES  COORDINATES 


Fig.  3(a).  Update  routine  I. 
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|  5- 4 ?- 103 75  | 


INPUT  FROM  CONDITIONAL  INTEGRATION  ROUTINE  II 


OUTPUT  IN  RADAR  OUTPUT  IN  RECTANGULAR 

COORDINATES  COORDINATES 


Fig.  3(b).  Update  routine  II. 
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)’]  s  covariance  of  the  error 


-k+l,  k  =  L  '(-k+l  -k-M,k)(-k+l  -k+ 1 ,  k 

in  the  estimate  x,  .  , 

—k+l , k 

5k  =  E  [(2£k+ 1  -Sk+1)  <ik+1  —  £k+ 1 )’  1  =  covariance  matrix  for  the 

estimate  at  time  t, 
k 

The  various  blocks  in  Fig.  1  are  described  below. 

A.  Initial  Conditions 

It  is  assumed  that  initial  conditions  for  the  state  estimate  and  the  covariance  matrix  of  that 

initial  state  estimate  are  known  at  some  time  t  ;  it  is  at  this  time  that  the  estimation  routine 

o 

starts.  The  first  observation  after  (or  at)  t  will  produce  the  first  estimate.  The  index  k  keeps 
track  of  the  estimates  x.  ;  the  zero  estimate  x  is  simply  the  initial  estimate  of  the  state  (i.e., 
prior  to  any  observation  after  or  at  t  ).  The  parameter  EKRAN  is  included  to  allow  the  same 
program  to  be  used  for  error  analysis;  it  is  set  equal  to  one  if  the  routine  is  used  for  error 
analysis  and  to  zero  otherwise. 


B.  Conditional  Integration  Routine 

In  the  conditional  integration  routine,  a  predictor- corrector  method  for  numerical  integra¬ 
tion  is  used.  The  specific  method  chosen  is  described  in  Sec.  VI  and  will  not  be  discussed  here. 
However,  certain  modifications  had  to  be  included  to  permit  observations  at  any  time  rather 
than  only  at  an  integral  multiple  of  the  step  size  in  the  numerical  integration.  The  routine  is 
equally  well- suited  for  real-time  estimation  and  post-flight  analysis.  If  the  observation  times 
are  known,  they  can  be  included  as  data  in  the  program.  For  real-time  analysis  the  integration 
will  probably  be  one  step  size  behind  time;  i.e.,  the  state  equation  is  integrated  one  step  at  a 
time  only  after  a  check  is  made  that  no  observation  occurred  during  the  time  interval  [V,  C  +  A^], 
where  V  is  the  time  to  which  the  state  equation  has  been  integrated,  and  A^  is  the  step  size  for 

the  integration  in  the  interval  [t  ,  t,  .].  If  an  observation  is  made  at  t,  .  (  [t  ,  t  +  A,  ],  the 

K  K+  1  K+  1  J  J 

integration  is  carried  forward  in  time  with  a  step  size  —  C.  Because  the  predictor-corrector 
method  chosen  requires  a  constant  step  size,  a  slightly  modified  predictor  has  to  be  used.  In 
particular,  this  is  the  same  predictor  as  the  one  needed  to  start  the  numerical  integration 
algor  ithm.  The  index  j  keeps  track  of  the  number  of  integration  steps,  and  the  index  (  is  used 
to  exit  from  the  loop  when  an  observation  is  received.  The  reason  for  having  two  slightly  dif¬ 
ferent  routines  [Figs.  2(a)  and  (b)]  is  given  below. 


C.  Update  Routine 

The  function  of  this  routine  is  to  update  the  state  estimate  and  the  covariance  matrix  when 
an  observation  is  received.  Both  the  updated  state  estimate  and  the  covariance  matrix  are  ob¬ 
tained  for  the  rectangular  coordinate  system.  Since  it  may  also  be  of  interest  to  have  these 
quantities  in  radar  coordinates,  such  a  conversion  has  also  been  included  in  the  routine. 

The  updating  of  the  state  vector  is  conditioned  upon  the  parameter  ERR  AN.  If  ERR  AN  -  0, 
the  routine  is  used  for  estimation,  and  hence  the  state  estimate  will  be  updated  using  the  current 
observation.  When  an  error  analysis  is  performed,  i.e.,  ERRAN  =  1,  the  evolution  of  along 

a  given  trajectory  and  for  a  given  set  (Rk)  is  found.  This  is  accomplished  by  letting  xk+1  x.k4  {  k, 
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as  shown  in  Figs.  3(a)  and  (b).  These  figures  represent  alternate  ways  of  solving  the  estimation 

4  S  -i  T 

equations  as  given  by  Mowery  and  Cox.  In  Fig.  3(a),  </  =  { 4*  )  is  computed  directly,  whereas 

in  Fig.  3(b),  T  is  computed  first. 

Section  III  and  Refs.  4  and  3  give  derivations  for  the  state  estimates  and  the  appropriate  co¬ 
variance  matrices.  Figures  2(a)  and  (b)  show  the  conditional  integration  routines  that  are  used 
with  the  updating  routines  of  Figs.  3(a)  and  (b),  respectively.  The  functional  form  of  all  matrices 
and  vectors  used  in  the  actual  algorithm  is  given  in  Sec.  V. 


V.  ST  AT  I :  EQUATIONS  AND  MATRICES  DEFINED  FOR  THE  BALLISTIC 
MISSILE  RE-ENTRY  PROBLEM 

A.  Dynamical  Equation  f(x) 

A  spherical  earth  is  assumed  with  the  radar  at  zero  altitude.  The  x-y-z  coordinate  system 
is  centered  at  the  radar  with  the  x-y  plane  tangent  to  the  earth  (x  east,  y  north)  and  z  vertical 
to  the  x-y  plane. 

dt  =  fi(*>  =  * 
d  v  ..  ,  . 

7ft  =  C**  =  -v 

dZ  r  /  s 

■fit  r  ‘3(i)  =  z 

(jt  f^(x)  =  ( WZ  —  g^)  x  —  ^  (xz  4  y-  +  try  x  +  2wy  sin<£  —  2wz  cos <p 

dv  ,.,,.2.2  ,  p  ,.2  .2  -2,1/2  .... 

■  t5(x)  =  (W  sin  </>  -  gc)  y  -  js  (x  +  y  +  z  )  '  y  -  2wx  sin<p 

2  2 
*—  w  z  sin  <p  cos  </)  —  w  ro  sin  <p  cos  <p 

dz  .....  2  2  .  p  2.2  .2.1/2  .  2 

—  t^(  x)  -  (w  cos  ip  —  )  z  —  (x  +  y  4  z  )  z  —  w  v  sinv  cos  ip 

2  2 

+  2\vx  cos  lo  4  r  (w  cos  q:  -  a  ) 
o  c 

L(x)  =  depends  on  the  specific  assumption  on  the  functional  dependence 


of  ft  (-  0  for  ft  constant) 
(_» 

m 

go  =  ,  2  7  2  2,3/2 

(x  4  y  4  z  ) 

-  latitude  of  radar 


r  radius  of  earth 
o 


p  -  weight  density  of  air 
ft  =  weight-to-drag  ratio 
w  =  earth's  angular  rotation  rate. 
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B .  \  0£(  x )/  dx  M at r i x 


al  1 

‘  ai2 

"  ai3  = 

=  0 

al4  =  1 

al5 

ai6 

=  0 

a21  "  a22 

=  a23  =  a24 

a25 

1 

• 

a26  ° 

a31 

=  a32 

a33 

a34  = 

a35  =  0  ’ 

a36  1 

a41 

=  (w2 

-gc)“ 

9gc 
'  x  9x 

.,  .2  .2 
-  x(  x  +  y  + 

.2,1/2  a 
z  >  j* 

4  2 


4  3 


44  2/3 


dJc 

c)y 

...2  .2  .2. 
-  x(x  4  y  4  Z  ] 

1  2 

1 

0 

ay 

1  2/3  j 

f*c 

dz 

-  X(X2 

.2  . 2. 
1  y  +  z  ) 

1/2 

a 

az 

/  P  X 
(2£' 

5  (X2 

.2 

4  V  4 

.2  1/2 
z  )  - 

X  - 

X 

(x  4  y  4  z  ) 


p  xv  ~ 

~  ~  T7T  - t - »r“ - TT^  4  2\V  Sin  <£ 


45  ~~2^  (x2  +  y2  +  zV  2 


P  XZ  ~ 

a  „  y  ^ ^ —  2w  cos  <p 


46  '  2/3  .  .2  7  .  2  .2.1/2 

(x  4  y  4  z  ) 


a  agc  ...2  ■2J  .2.1/2  a  .  p 

a5 1 3-y  -5?  -y<x  +y  +  *  >  9^ 


,2.2  .  0gc  ...2  .2  .2.1/2  a  ,  p  . 

a52  Mw  sin  <p  -  gc>  -  y  —  -  y{x  +  y  +  z  )  (  $  ) 


3g, 


"*c  .. .2  .2  .21/2  9  ,  p  .  2  . 

=  -  y  -sr —  yu  +y  +  z  )  tz  (£>)  - 


dz(fp)-'N  sin?  cos« 


a54  =  “I - .2^  .2  1/2  -  2w  sin* 

(x  4  y  4  z  ) 


l55  =  -  Z/5 


.2 


.2  .2  ,  -2,1/2 


72  . 

(x  4  y  4  z 


v  . .  l  .  ^ 

i  .2.1/2  +  (x  +  y  +  z 


56 


vz 


P  , .  2  . 2  .2,1/2 

'  (x  4  V  4  Z  ) 


,  ,  .  agc  ...2  .2  .2.1/2  a  ,  p 

a-  =  -(«•„  +  z)  -ST  7.(x  +  y  +  z  )  '  5-  (  £ 


61  v  o 


Ox 


£  _  .,,.2  ,  ,.2  x  ,2,1/2  _9 

3y 


a. ,  =  — (r  +  z)  — - z(x  +  v  +  z  ,  . 

62  '  o  8y  '  "  9y 


ax  '2?' 
{2p)  - 


2  . 

w  sin  <r  cos 


2  2  ,  .  J  ,  9gc  . . . 2  .2  . 2.1/2  a  ,  p 

aAi  <w  cos  <p  -nJ~  <r„  +  z>  -?r  -  z<x  +  y  +z  >  a7  <  ^ 


63 


o  Oz 


dz  v  2/3 


17 


a,  .  -  2w  cos  <7  —  o T  rz 

64  v  2/3  . 2  .2  2,12 

(x  +  y  4  z 


65 


2/3  772  .2  .i  1/2 

'  (x  4  y  4  z  )  ' 


.2 

z 


772  72  .2,1/2 

(X  4  V  4  Z  ) 


66  2/3 

The  dimension  n  of  A  as  well  as 

a  ,  i  =  1 ,  2,  .  .  .  n;  j>6 
i.l 


. 2  2  .21/2 
4  {  X  4  V  4  Z  ) 


a.  ,  i  <  6;  j  =  1,  2,  . .  .  n 


will  depend  on  the  specific  functional  form  assumed  for  a  =  1//3.  The  additional  elements  of  A 
for  a  -  constant  are  (n  =  7,  with  the  seventh  state  variable  =  o): 


a  1  7  =  ° 


a27  -  ° 


a37  =  ° 


P  ,-2 

.2  ,  .  2 

1/2  . 

I  O 

4 

y  +  z  ] 

>  x 

P  ,  •  2 

.2  . 2 

1/2  . 

T  (x 

4 

y  4  z  ] 

1  y 

P  <  •  2 

.2  . 2 , 

1/2  . 

7  (X 

4 

y  +  z  : 

)  z 

a  ^  = 

a 

=  a  „ 

—  ci  _ 

72 

’3  74 

75 

(\  Observation  Relation  h(x) 


2  2  21/2 

hj(x.)  ~  r  =  range  =  (x  4  y  4  z  ) 


h^(x)  r  o  -  azimuth  =  tan  1  7- 


h^(>0  =  c  -  elevation  -  tan  *  — ^ — Z  2  j~/’2 

(x  +  y  )  ' 


.  .  ,  -  xx  4  vy  +  zz 

^4  —  =  r '  7^ — 2  "-71T2 

(X  4  y  4  z  ) 


D.  H  IVIatrix 


h.  =  ^ 


11  ax  '  2  2  2. 172  "  r 

(x  +  y  +  -t  ) 


h  =  §£  ,  _ 2 _ _ 

12  oy  ,  2  7  2'  2.1/2 

J  (x  4  y  4  z  ) 


h.„  =  £  - 


(x  4  y  4  Z  ) 

7 

(x"  4  yZ  4  z")1 


h14  =  h15  =  hl6  ■-  0 
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da 

y 

121 

dx 

" Z 

x  +  y 

da 

—  X 

122 

dy 

2  2 
x  ^  y 

12  3 

h21 

h25  =  h26  =  0 

dc 

—  xz 

-  xz 

1 3 1 

dx 

,  2  ,  2~  2  ,  2  21/2  ■ 

(x  fy  +z)(x  +  y  ) 

2 

r 

(X2 

2,1/2 
+  y  ) 

d€_ 

-vz 

-yz 

*32 

dy 

~2  Z  2'  "  2  2.1/2  ' 

(x  +  y  +z)(x  +  y  )  ' 

2 

r 

(X2 

2,1/2 

+  y  ) 

dc 

,  2  2,1/2  .  2  2,1/2 

l33 

dz 

x2  +  y2  +  z2  r2 

l34 

= 

h35 

- 

h36  =  0 

dr 

2  2  2 

x(  x  +  v  +  z  )  —  x(  xx  +  yv  + 

zz) 

xr  —  rx 

l41 

dx 

1  2.3/2 

( x  +  y  +  z  ) 

2 

r 

dr 

2  2  2  .  . 
y(  x  +  y  4  z  )  -  v(  xx  4  yy  4 

zz) 

yr  -  ry 

l4  2 

dy 

"“Z  2  2,3/2'" 

(x  +  y  +  z  ) 

r 

dr 

2  2  2 

z(x  4  y  4  z  )  —  z(xx  4  yy  4 

zz) 

zr  —  rz 

l4  3 

dz 

",  2  Z  2,3/Z" 

(x  4  y  +  z  ) 

2 

r 

dr_ 

x  x 

l44 

dx 

~T~  2  7  2,1/2  ■  r 

(x  4  y  4  z  ) 

dr 

y  _  y 

l45 

dy 

72 2  2,1/2  ~  F 

(x  4  y  4  Z  ) 

dr 

z  z 

l46 

dz 

7~2  2  2,l/2  “  r 

(x  4  y  4  z  )  ' 

K .  Relation  Between  Coordinate  Systems  g(x)  + 

,  ,  ,  2  ,2  j  2,1/2 

g1(2L)  =  r  =  (x  +  y  +  z  ) 


g2(x)  =  cv  =  tan'1  X 


g3(x)  =  f  =  tan'1  2  7  2.1/2 

(x  +  y  ) 


/  \  -  xx  +  yy  +  zz 

*4(*>  =  r  =  -2 - r  2, i/Z 

(x  +  y  +  z  ) 


t  g(x)  as  defined  here  bears  no  relation  to  g(x)  in  (3-1). 
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g5(x)  « 

(x  +  y  ) 


g.(x)  =  t  =  (,x  +  r"1 

fe6v-’  ,  2  2  2,  T77 

( x  4  y  4/  (  X  4  y  )  ' 


B  -  r)g( x)/ r'K  Matrix 

b  -  -  . 

1  1  r 


12  r 


13  ~  r 


b14  ° 


b15=  ° 


bl6  =  0 


>i  ”  2  2 

x  +  y 


12  2  2 
X  +  Y 


b23  =  0 


b24  -  ° 


b,-  =  0 

2s 


b26=  0 


-  xz 

'  3 1  =  2  2  2  1/2 

r  (x  +■  y  ) 


b34  =  ° 


32  ~  2,  2  2.1/2 

r-  (x  4  v  ) 


b35  =  0 


U  _  X  4  yfc 

b33  — r~ 


b36  =  ° 


41 


r  x  —  x  r 

- - — 


b  =  ry  -  yr 
42  2 
r 


V7.  -  zr 


A  3 


44  '  r 


45  “  r 


46  '  r 


2xor  4  y 

bs  i  =  ~i — i 

X  +  V 


x  ~  2o*y 

3c,2  z  ^ 

x  4  y 


b53  =  0 


b54  ’  T  2 


55 


x  +  y 


b56  =  0 


6 1  2  .  2 
x  4  V 


b62 


2  2 
x  4  y 


r  b31  2.  2"  2.1/2 

r  (x  4  y  ) 

—  —  b  — 


r  32  2.  2  2.1/2 

r  (x  4  y  ) 


v  _  xf  _  r  . 

°63  “  2  r  d33 

r 


b64  =  b31  =  2.  2  2.1/2 

r  ( x  4  v  ) 


b, c  =  b_ 


-yz 


J65  32  2  2  2  1/2 

r  (x  +  y  ) 


b66  *  b33 


x  4  y 


VL  NUMERICAL  INTEGRATION  OK  THE  EQUATIONS  OF  MOTION 

In  Sec.  II  we  considered  a  system  described  by  the  nonlinear  state  differential  equation 

x=f(x)  (6-1) 

where 


x(t  )  =  x 

—  o  — o 
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and  x  is  an  n-dimensional  state  vector.  Discussion  was  limited  to  the  general  concept  of  linear¬ 
izing  the  nonlinear  equations  about  a  nominal  trajectory.  In  this  section  these  concepts  will  be 
extended  to  obtain  a  specific  algorithm  for  solving  the  system  of  equations  presented  in  Sec.  V. 
This  algorithm  is  then  used  to  compute  the  x^  ^  ^  or  conditional  estimate  term  of  (3-23). 

In  addition,  there  is  a  requirement  to  update  the  covariance  matrix  (2-9)  or  (3-25).  The 
integration  algorithm  given  here  satisfies  this  need.  Part  D  presents  details  concerning  com¬ 
putation  of  the  incrementally  linearized  transition  matrix  used  in  updating  covariances. 

Let  the  solution  of  (6-1)  with  the  initial  condition  be  denoted  by  x°(t).  There  are  a  number 
of  schemes  available  which  use  numerical  integration  to  obtain  the  solution  x  (t).  Since  in  the 
present  ease  it  will  also  be  necessary  to  find  a  solution  to  the  linearized  incremental  differential 
equation 


Of 


dl  i  w  =  ^ 


x°(t> 


6x(t) 


(6-2) 


only  methods  which  make  use  of  higher  than  first  derivatives  (in  particular,  second  derivatives) 
of  \(t)  at  various  times  will  be  considered. 


A.  Taylor  Series  Predictor 

The  simplest  way  of  using  the  knowledge  of  x  is  by  means  of  a  Taylor  series  expansion. 
Assume  that  the  value  of  x(t)  is  known  at  time  t^.  For  simplicity,  it  will  be  assumed  that  x(t) 
will  be  found  at  equally  spaced  points  along  the  time  axis.  Let 

A  =  t.  —  t,  =  constant  for  all  k 
k  k- 1 

xk « *<y 


d\ 

-k  '  TTT 


t=t, 


K*k> 


then 


x,  .  x,  +  x,  A  +  x,  — =-  +  higher  order  terms 

-k+1  — k  “k  ~k  2 


Of 

-k  =  9x 


*k 


9f 

-k  =  9x 


S*k> 


Of 


^k+i  =  *k  +  I(ik)  A  + 


_f ( ,  )  -j-  +  higher  order  terms 


Hence.  x^i  can  approximated  by 


Of 

^k+i  :  +  ^k>  A  4  al 


*k 


^k>  %■ 


(6-3) 


with  the  i^1  component  of  error  E.  -  (A^/3)  x.^^(a.)  <  a.  < 
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B.  Predictors  RasecI  on  More  Than  One  Point 


II  only  the  values  of  xk  and  its  derivatives  are  to  be  used,  then  the  Taylor  series  expansion 
is  the  best  polynomial  approximation.  However,  by  using  appropr  iate  previous  values  of  x  and 
its  derivatives,  an  improvement  over  (6-3)  is  possible.  A  number  of  such  predictors  exist  which 
ran  be  generated  in  several  ways.  Four  predictors  art'  given  below,  together  with  the  derivation 
for  the  predictor  used  in  the  algorithm  described  in  part  1). 

The  following  predictors  involve  onI\  x.  , ,  x.  . ,  x,  ,  x,  ,  x.  ,  x,  • 

*  1  ■  —k-l  -k-l  -k  -k  -k  — k- 1 


{  3  x. 


-k+1  -k  2  l  -k-l  "  -k;  '  12  (17-k 


-  xt  )  + 


A 


4 


(6-1) 


-k+1 


A 


i  ‘  4-i1  + 1  (74-i  "4* 4  it  (J34 4  154-i> 


(6-5) 


-k+1 


-k-l 


A 


(44 
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\  predictor  using  xR_2,  xk_r  Ak.r  %  rs 


(6-6) 


-k+1  "  -k-2 


3<-k  "  — k-l  ^  f  A  (-k  -1 


k-l 


(6-7) 


To  derive  (6-6)  we  use  the  Taylor  series  expansion  for  Ak4|>  *k  j  an<*  first,  and  second  deriv¬ 
atives.  liquations  (6-4),  (6-5),  and  (6-7)  can  be  derived  similarly,  and  in  fact,  by  using  the 
same  method,  an  infinite  variety  of  predictors  can  be  generated.  The  various  series  are  |assum- 
ing  x^  \s)  continuous) 


—k+1  ’  “k 


A  - 


— k 


A 


% 


A5  (3)  A4  (4) 
3:  -k  +  4'.  -k 


B4 


(6-8) 


.th 


where  the  i  component  of  H  ^  is  given  by 

,  1  f’tk+ 1  (5),  .4  . 

1-4  i  =  ~  J  xi  (S)  (tk.H~S)  ds 


and 


;  4x,(3)4  £  x.U>  +  „• 


-k-l  -k  A-k  4  2  -k  3V  -W  '  4'.  -k 

r 

t 


i  r'k-i  (5)  ,4  . 

IS4  lj  :  \  («)  <‘k_l  ds 


k 


A4-i  =  ir  43|-%-44,*aV 

isv'r  £fk"'  xi'5,i“>  "k-i  - s|3  d* 

k 

a\  .  +  x<4)+RV' 

-k-l  — k  — k  2:  — k  —4 


(6-4) 


(6-10) 

(6-11) 


(6-12) 


(6-1  3) 


(6-14) 
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m'h-  xi(5)(s)  (tk-i  - s)2  ds 


(6-15) 


Multiplying  (6-10)  by  -  1,  (6-12)  by  —2,  (6-14)  by  —  2/3,  and  adding  them  to  (6-8)  yields 

x.  x,  -  x,  ,  ~  2Ax  ,  -  i  A2x.  I  A2x.  +  R  .  -  R'  -  2.R7  -  |  R7" 

“k+1  “k- 1  “k- 1  3  — k-1  3  — k  —4  “4  —4  3  “4 


*k+l  =  *k-l  +  2A4-1  +  f  a2(4-1  +  2*k>  +  —4 


(6-16) 


where 


R*  =  R.-R'  -  2RV  -  •=•  R'" 
—4  —4  —4  —4  3  —4 


tS^lj  =  4T 


Ptk+l  (5),  ,  <t  .4  , 

I  x!  (s)(tk+i-s)  ds 

K 


x!5\s)  l(tk.,  -  st4 


+  8A(t,  .  —  s)  +  8 A  (t,  .  —  s)  ]  ds! 

k-1  k-1  1  \ 


(6-17) 


Consider  now  the  term 


[<tk_4  -  s)4  +  -  S)3  +  8A2(tk_1  -  S,2, 


(6-18) 


In  (6-18)  it  is  clear  that  since  (t^  ^  -  s)  <  A 


8A(tk_1  -  S)3  +  8A2(tk_1  -  s)2  >  0 


\-i  454tk 


Hence,  (6-18)  is  non-negative  in  the  interval  of  integration.  13y  using  the  mean  value  theorem 
of  integral  calculus,  (6-17)  can  be  written  in  the  form 


mb-  7;  xi(5)(ai>  f 


k+1  ,,  .4  , 

(tk+i  ~ !,)  ds  + 


_l  (5),  .  r* 

41  xi  ^  i  Jt 


k 

k-1 


x  [(t.j  -  sV  +  8A(tkl  -  s)  +  HA^(tk_  4  -  s)  |  ds 

A5  (5).  ,  ,  A5  (5)  ...  8X58X5 

=  —  x.  (a.)  +  —  x.  (^j)  (1  -  —  +  “T“ > 


iasii3Trixi(5)<ffi,  +  Txi(5Vi 


(6-191 


where  tk<  a.  <  tk+J;  tk  l  <  ip .  <  tk  for  i  =  1,  2,  ...  n.  Assume  now  that 
x.(5)(o.)  =  x.<5)(„>.)  +  h. 


then 
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111:,!;  5T5T5  I3x.(5\¥i»  4  3h  4  13x.(5)< «..)] 


A  [16x;(5)(c,)  4  3h.| 


5!  x  3  i  ^  i 


16A  ,  (U  x  3 

,  / n  x.  {(p . )  -t  77-  h . 

360  1  1  1  lb  r 

but  x^V)  +  3/  lb  Ir  will  be  a  value  between  x/^U  . )  and  x.^(o\). 
there  exists  a  il  such  that 


(a.).  Hence,  because  of  continuity, 
11 


( R *  1 .  =  T  \[  V>  A  5 

1 — 4  1  45  1  1 


for  i  =  1 ....  n 


(6-20) 


Hence,  if  the  predictor  (6-6) 


-k4 1  ”  -k-1  +  2A-k-l  3  A  (^k-l  +  2-k' 


»s  used,  the  remainder  (or  error)  term  is  given  bv  (6-20). 


C.  Correctors 


By  means  of  iterations  it  is  possible  to  improve  the  ac  curacy  of  numerical  integration.  I  r»r- 
rnulas  used  for  the-  iterations  arc*  called  correctors,  since  they  all  require  a  starting  value  which 
is  then  "corrected"  in  subsequent  iterations.  To  obtain  such  formulas  a  procedure  identical  to 
that  in  the  previous  section  t  an  be  used.  In  particular 


2  3  4 

,  A  .  A  ..  ^  A  (3)  A  (4)  t  „ 

2k+i  "*k  +  A*k  +  T  +  3T  5k  +“^k  +  —5 


IBs1!  h£k 


k" 


(6-21) 

(6-22) 


a  nd 


.  •  ,  ■  ,  .  2..  A  1  (3)  A  *  (4)  ,,, 

Ax  Ax  +  A  x,  +  —r  x  +  x  +  R_ 

— k+1  “k  ~ k  2  — k  3;  — k  —5 


(6-23) 


L&di  T7  A  \ 


k+1  (5),  w*  \ 3  , 

x.  s)  t.  ,  -  s)  ds 
1  ks  1 


(6-24) 


.2..  .2..  .  i  (  3)  A4  it) 

A  ^k4i  A  ik  ’  A  ^k  T  T  +  ^s 


(b-'25) 


Hi 


k4t  (5).  .  .2  , 

x.  (s)(tk+1-»)  ds 


(6-26) 


Multiplying  (6-23)  by  -l/2,  (6-25)  by  1  12,  and  adding  them  to  (6-21)  yields 


x,  .  .  x.  +  %  (x.  +  x.  . )  +  -Jr-  A 2(  x.  -  x .  ,  -  )  +  R  t 
— k+1  — k  2  '-k  — k+1  12  -k  -k+1  —6 
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(6-27) 


(6-28) 


—  5  —  S  2  — 5  +  12  —s' 


R+1  v(S)(«)  l(tk+1  "  *>4  -  2A(tk+1  -  .s)3  +  A3(tk  -  s)2]  ds 


i-5'i  =  i  [ 

V.  f k+1  xiS)<SH(tk+1--s)2-A(tk+1-s)l2ds 


\gain  applying  the  mean  value  theorem  of  integral  calculus  to  (6-28)  yields 


ISIli  IT  !f’«V  (t  -  4  *  ir) 


uaru  ™  C'V 


where  t,  ^  0.  t.  .  for  i  -  1, 
k  i  k+1 


Hence,  if 


1  2 

-k+l  -k  ’  2  '-k  — k+ 1 '  +  Tz  A  (-k--k-tt' 


(6-29) 


(6-30) 


^k+i =  %  +  t  <4 +  4 

is  used  as  the  corrector,  the  remaining  (or  error)  terms  are  given  by  (6-29). 

1).  Algorithm  for  the  Numerical  Solution  of  a  Differential  Equation 

It  is  now  possible  to  give  a  complete  algorithm  for  the  solution  of  the  differential  equation 
x  =  f(x) 


x(0)  =  x, 


(6-31) 


The  predictor -cor rector  method  is  used,  with  the  corrector  being  of  the  form  of  (6-30).  For 
the  predictor,  (6-6)  was  selected.  Recall  from  Sec.il  that  it  is  also  necessary  to  evaluate  the 
transition  matrix  for  the  linear  time-varying  equation 


d  , .  ,  3-f 

Hi =  (<>s)  ^ 


X°(t) 


6x 


(6-32) 


in  order  to  update  the  covariance  matrix.  This  is  easily  incorporated  into  the  present  algorithm. 
The  flow  chart  (Fig.  4)  shows  the  sequence  of  operations.  Since  (6-6)  requires  x^  j,  x^  has  to 
be  evaluated  by  other  means.  As  indicated  in  the  flow  chart,  this  is  done  by  using  a  Taylor 
series  expansion  about  x  , 

i  “O 

In  the  flow  chart  symbolism,  $  denotes  the  transition  matrix  of  (6-32),  For  evaluating  4>, 
the  assumption  is  made  that  during  the  interval  of  time  of  length  A,  Of/ 0x_ will  be  essentially 
constant.  Hence  if  the  notation 


Of 

Ox 


(6-33) 


is  used,  the  transition  matrix  at  the  point 
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Fig.  4.  Flow  chart  of  algorithm. 
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*  ^k'V 


(6-34) 


can  be  written  as 


4> 


A,A  A.  A  A  A 
k  k-1  o 

e  e  .  .  .  e 


(6-35) 


1  his  follows  from  the  fact  that  with  the  assumption  of  constant  Of/ Ox  during  the  interval  A,  (6-32) 
represents  a  piecewise  constant  system.  It  is  also  easy  to  see  that 


0(t 


k+1 


to) 


0(tk+l’  V 


♦“k-V 


A.  A 

—  k+1 


♦'VV 


(6-36) 


Equation  (6-36)  updates  at  each  new  t^.  In  order  to  evaluate  exp[A^+^A],  a  power  series  ex¬ 
pansion  of  the  form  exp  [A ^A]  -  1  +  A^A  +  A^(A^/2)  +  (higher  order  terms)  was  used,  with  the 
higher  order  terms  neglected.  For  A  sufficiently  small  (which  it  has  to  be  for  the  assumption 
3 f/  9 \  constant),  this  approximation  will  be  valid.  However,  for  increased  accuracy  we  may 


(1)  Drop  the  assumption  (Of/dx) 


x°(t) 


constant  during  the  interval  A.  As  an 


approximation  it  could  be  assumed  for  example  that  in  the  interval 

[t,  ,  t,  . ,  ] .  (6-32)  takes  the  form 
K  K+1 


57  [6x(t)]  -  \ 


^k+l"^ 


k 


(t  -tk)  6x(t) 


(6-37) 


(2)  Evaluate  the  transition  matrix  0(t,  . ,  t,  )  =  cb,  to  powers  in  t  higher 

than  the  second. 


It  will  be  seen  that  in  the  flow  diagram  the  correction  iteration  loop  is  made  conditional  upon 
the  size  of  the  difference  between  successive  approximations.  This  need  not  be  incorporated, 
although  it  is  worthwhile  to  obtain  and  record  the  difference  between  the  predicted  value  and  the 
value  found  by  iteration.  This  will  become  clear  in  the  next  section. 


E.  Estimate  of  the  Truncation  Error 

In  the  algorithm  described  previously,  (6-6)  was  used  as  the  predictor,  and  (6-30)  was  used 

to  obtain  the  final  value  of  x.  , .  If  we  denote  the  difference  between  the  corrected  and  the  pre- 

— k+1 

dieted  value  of  x,  ,  by  C  ,  then  C  represents  the  difference  in  the  truncation  error  of  (6-  30) 
—k+1  J  — o  — o  1 

and  (6-6)  or 

C  =  predicted  value  —  corrected  value 
— o  1 


«y,  -  4 

A5 

720 

x.(5V) 

1  1 

(6-38) 

where  it  will  be  recalled  that  t,  ,  <  ip- 

K- 1  1 

tk+l 

and  tkOj<tk+1  or 

+  a! 

720 

(6-39) 

where  x.^(</j.)  -  x^(/3.)  was  expressed  as 
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)=  ^  > 
1  Jfl 


T’o)  ,1s  x.{f,)(a.)  «■.  -  0.) 


and  min{;'  .  ,£.)^  a  <  maxu.,0.).  Hence 


and 
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&  A'X'  * 


llll'i 


M 


,(5) 


(5) 


1  .(6), 


?20  |x.  ('V  X’  ^  i 1  ~  TT  xi  («.)(*s-0.)l 


A 

720 


31  l 


32  (6) 

31  V 


where  it  is  assumed  that 


Thus 


,4  M||  Ui-/?iUA6 


o  M  If 


x'°V)|  <  M  < 


k-1 


t  <  t 


k+1 


m  -  if 


Prom  (6-4)  it  follows  that 


^5  31 


V 


l-  1 


if 


2NMA 

1395 


{6-40) 


(6- 11) 


with  an  error  of  the  order  A  . 

In  general,  A  will  be  chosen  small  enough  so  that  only  one  iteration  is  required.  Conse¬ 
quently,  a  good  estimate  of  the  truncation  error  {with  an  accuracy  of  order  C^)  can  be  obtained 
by  recording  for  the  first  pass  through  the  iteration  loop  {see  Fig.  4). 

F .  C  on v e r ge  n C e  o f  1 1  e  r a t i on  s 

Previously,  it  has  been  tacitly  assumed  that  the  iterations  indicated  in  the  algorithm  will 
converge.  To  find  under  what  circumstances  this  will  be  true,  consider  the  i^1  iteration.  Mere 

superscripts  will  denote  the  value  of  x,  .  obtained  at  the  respective  iteration,  i.e., 

— 1 


i+  l 


i 


2,.. 


•'1  A  X  +  -7  ( X.  +  X,  4  )  +  T7  A  (x.  —  X 
-k+1  — k  2  -k  -k+1  12  — k  - 

represents  the  value  of  after  the  i^1  iteration.  Then 


^kti1 


i+ 1  i 
-k+1  -k+1 


A  .  .  i 


i-1 , 


1  A  2...  i-1  ..  i 


2  (-k+l  —k+1  ^  +  12  A  (-k+l  —ki  1 J 


(6-42) 


Lot*j  — k+  i  4:1 then 

t  A  d  A  a2  d2 

~i+l  2  dt  “i  12  .2  (-i] 

dt 

in  order  to  obtain  d6./dt  and  d  6./dt  in  terms  of  6.,  the  differential  equation  of  motion  (6-31) 
will  be  used. 

-k+l  -(-k+l* 

Hence 


.  ,  3f 

.1  .1-1  Ci  1  v  1-1  V 

-k+l  -k+l  =  k+l  -  — k+l  '  3x 


x=«i 


/  i  i-1. 
—k+l  -k+l 


(6-4  i) 


,  8f 

dt  <4  = 


where  Df/c)x_is  assumed  continuous  in  the  interval  of  consideration,  and  £  is  a  value  of  \  such 
that  (6-43)  is  satisfied.  To  simplify  notation,  the  definition 


c)f 

£<4  -  07 


-£i 


(6-44) 


is  introduced.  To  obtain  the  variation  in  the  second  derivative,  the  expression  for  ^  will 
first  be  obtained.  It  follows  from  (6-31)  that 


af 

^k+ 1  Ox 


0f 

4+i =  ^  -f(i) 


^k+l 


(6-46) 


-  Xk+1 


i  lence 


^k+i  =  i^k+i’ 
where  g(  •  )  is  defined  by 

c)f(x) 


g(x)  = 


0X 


f(  x ) 


(6-46) 


Therefore 


..i  ..  i-1 

x,  ,  4  —  x 


^g 


■k+l  —k+l  0x 


i  i-1 

<*k+i"  W 


(6-47) 


whore  it  is  a 


again  assumed  that  Og/Ox  is  continuous,  and  r/.  is  chosen  to  satisfy  (6-47).  Defining 


3g 

£(V  =  3x 


(6-48) 
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(6-47)  becomes 


,2 

"~2  <*!>  =  0  V  6.  . 

dt  1  -1  1 

1  i en oe,  ( 6  -  4 2 )  be  e om t' s 

5i+i  [ff(«i>-T?S(a,>]«i  ■ 

Equation  (6-49)  represents  a  vector  difference  equation  of  the  form 

6.  .  A  6  (6-50) 

-i+l  -l-i 

? 

A .  =  %  F(|.)  -  ^  G<»(.)  • 

If  all  the  o.’s  are  small  enough,  tiien  A.  may  be  assumed  constant  so  that  the  condition  for  stability 

becomes 

IxaJ  -v  1 

where  \  ^  are  the  eigenvalues  of  A  ^  A .  -  constant.  The  rate  of  convergence  will  also  be  deter¬ 
mined  by  how  close  the  magnitudes  of  the  eigenvalues  are  to  1.  Since  it  is  rather  tedious  to 
evaluate  A  ■  and  its  eigenvalues,  as  well  as  to  check  the  basic  assumption  that  .  is  in  fact  con¬ 
stant,  the  chief  value  of  (6-49)  is  to  show  that  if  F(£.)  and  0{  . )  are  bounded,  there  always  exists 
a  A  such  that  the  solution  to  (6-50)  is  asymptotically  stable,  i,e,. 

lim  <5  -  0 

n— 

and  hence  tlie  iteration  called  for  in  the  algorithm  will  be  stable  for  sufficiently  small  A. 
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